#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <cstring>

using namespace std;

// 类型定义
typedef vector<vector<double>> Matrix;
typedef vector<double> Vec;

// GridMap 结构体
struct GridMap {
    vector<int> hMap;
    vector<int> flagkxcar;
};

// fsg3d1 函数：生成 3D 网格
void fsg3d1(const Vec& d, const Vec& zmin, const Vec& zmax, const Matrix& pilemin, const Matrix& pilemax,
            Matrix& zMap, vector<int>& idMap) {
    double deltaX = (zmax[0] - zmin[0]) / d[0];
    double deltaY = (zmax[1] - zmin[1]) / d[1];
    double deltaTh = (zmax[2] - zmin[2]) / d[2];

    double xmin = pilemin[0][0], ymin = pilemin[0][1];
    int xn = ceil((pilemax[0][0] - pilemin[0][0]) / deltaX);
    int yn = ceil((pilemax[0][1] - pilemin[0][1]) / deltaY);

    vector<double> x1(xn), y1(yn);
    for (int i = 0; i < xn; ++i) x1[i] = xmin + (i + 1) * deltaX;
    for (int i = 0; i < yn; ++i) y1[i] = ymin + (i + 1) * deltaY;

    vector<int> ith(d[2]);
    for (int i = 0; i < d[2]; ++i) ith[i] = i + 1;

    vector<vector<vector<double>>> tth(xn, vector<vector<double>>(yn, vector<double>(d[2])));
    for (int i = 0; i < xn; ++i) {
        for (int j = 0; j < yn; ++j) {
            for (int k = 0; k < d[2]; ++k) {
                tth[i][j][k] = zmin[2] + (ith[k] - 0.5) * deltaTh;
            }
        }
    }

    zMap.resize(xn * yn * d[2], vector<double>(3));
    idMap.resize(xn * yn * d[2]);

    int idx = 0;
    for (int i = 0; i < xn; ++i) {
        for (int j = 0; j < yn; ++j) {
            for (int k = 0; k < d[2]; ++k) {
                zMap[idx][0] = x1[i];
                zMap[idx][1] = y1[j];
                zMap[idx][2] = tth[i][j][k];
                idMap[idx] = idx + 1;  // idMap 可能需要根据具体情况调整
                ++idx;
            }
        }
    }
}

// mindisobs 函数：计算每个点到障碍物的最短距离
void mindisobs(const Matrix& Z0, const Vec& gMap, const Vec& nz3d, const Vec& zmin, const Vec& zmax,
               Matrix& dis2obs) {
    int N = Z0.size();
    double dx = (zmax[0] - zmin[0]) / nz3d[0];
    double dy = (zmax[1] - zmin[1]) / nz3d[1];
    double lf = 3.5, lr = 3.0;
    int K = 3;

    vector<double> rf(N, 100), rb(N, 100);
    for (int k = 0; k <= K; ++k) {
        vector<double> xk(N), yk(N);
        for (int i = 0; i < N; ++i) {
            double vcos = cos(Z0[i][2]);
            double vsin = sin(Z0[i][2]);
            xk[i] = Z0[i][0] + k / double(K) * lf * vcos;
            yk[i] = Z0[i][1] + k / double(K) * lf * vsin;
        }

        // 计算当前坐标所对应的栅格位置
        vector<int> ix(N), iy(N);
        for (int i = 0; i < N; ++i) {
            ix[i] = ceil((xk[i] - zmin[0]) / dx);
            iy[i] = ceil((yk[i] - zmin[1]) / dy);
        }

        // 更新障碍物距离
        for (int i = 0; i < N; ++i) {
            int i2d = ix[i] + (iy[i] - 1) * nz3d[0];
            if (ix[i] >= 1 && ix[i] <= nz3d[0] && iy[i] >= 1 && iy[i] <= nz3d[1]) {
                rf[i] = min(rf[i], gMap[i2d]);
            } else {
                rf[i] = 0;
            }
        }
    }

    // 后车线计算
    for (int k = 1; k <= K; ++k) {
        vector<double> xk(N), yk(N);
        for (int i = 0; i < N; ++i) {
            double vcos = cos(Z0[i][2]);
            double vsin = sin(Z0[i][2]);
            xk[i] = Z0[i][0] - k / double(K) * lr * vcos;
            yk[i] = Z0[i][1] - k / double(K) * lr * vsin;
        }

        vector<int> ix(N), iy(N);
        for (int i = 0; i < N; ++i) {
            ix[i] = ceil((xk[i] - zmin[0]) / dx);
            iy[i] = ceil((yk[i] - zmin[1]) / dy);
        }

        // 更新后车障碍物距离
        for (int i = 0; i < N; ++i) {
            int i2d = ix[i] + (iy[i] - 1) * nz3d[0];
            if (ix[i] >= 1 && ix[i] <= nz3d[0] && iy[i] >= 1 && iy[i] <= nz3d[1]) {
                rb[i] = min(rb[i], gMap[i2d]);
            } else {
                rb[i] = 0;
            }
        }
    }

    // 计算前后车距离障碍物的最短距离
    for (int i = 0; i < N; ++i) {
        dis2obs[i][0] = rf[i];
        dis2obs[i][1] = rb[i];
    }
}

// getindex3d1 函数：获取 3D 网格的索引
void getindex3d1(const Vec& xx, const Vec& yy, const Vec& th, const Vec& nz, const Vec& zmin, const Vec& zmax,
                 vector<int>& k, vector<bool>& ir) {
    Vec dz = {(zmax[0] - zmin[0]) / nz[0], (zmax[1] - zmin[1]) / nz[1], (zmax[2] - zmin[2]) / nz[2]};

    vector<int> tth1(th.size());
    for (size_t i = 0; i < th.size(); ++i) {
        tth1[i] = fmod(th[i], 2 * M_PI);
        if (tth1[i] > M_PI) tth1[i] -= 2 * M_PI;
    }

    vector<int> id1(xx.size()), id2(xx.size()), id3(xx.size());
    for (size_t i = 0; i < xx.size(); ++i) {
        id1[i] = ceil((xx[i] - zmin[0]) / dz[0]);
        id2[i] = ceil((yy[i] - zmin[1]) / dz[1]);
        id3[i] = ceil((tth1[i] - zmin[2]) / dz[2]);
    }

    ir.resize(xx.size());
    k.resize(xx.size());
    for (size_t i = 0; i < xx.size(); ++i) {
        ir[i] = (id1[i] >= 1) && (id1[i] <= nz[0]) && (id2[i] >= 1) && (id2[i] <= nz[1]) && (id3[i] >= 1) && (id3[i] <= nz[2]);
        k[i] = id1[i] + (id2[i] - 1) * nz[0] + (id3[i] - 1) * nz[0] * nz[1];
        if (!ir[i]) k[i] = 1;
    }
}

// contiObsMap_dymap_G 函数：主函数，处理动态障碍物地图
GridMap contiObsMap_dymap_G(const Matrix& obs1, double dx, double dy, double rsafe, const Vec& zmin, const Vec& zmax,
                             const Vec& nz, const Matrix& obsnf, double Ahmap, const Matrix& pileRange, const Matrix& selectPile) {
    int obsnf_ = ceil(obs1[obs1.size() - 1][0]);
    Matrix obs(obs1.begin(), obs1.begin() + obsnf_);

    int n1 = nz[0], n2 = nz[1];
    Vec zmin_ = zmin;
    Vec zmax_ = zmax;
    vector<double> gMap(n1 * n2, rsafe + 4);

    double deltaX = (zmax[0] - zmin[0]) / n1;
    double deltaY = (zmax[1] - zmin[1]) / n2;

    // 处理障碍物
    for (const auto& obsk : obs) {
        double xK = obsk[0], yK = obsk[1];
        double minIX = ((xK - dx) - zmin[0]) / deltaX + 0.5;
        double maxIX = ((xK + dx) - zmin[0]) / deltaX + 0.5;
        double minIY = ((yK - dy) - zmin[1]) / deltaY + 0.5;
        double maxIY = ((yK + dy) - zmin[1]) / deltaY + 0.5;

        vector<int> iX_, iY_;
        for (int i = floor(minIX); i <= ceil(maxIX); ++i) iX_.push_back(i);
        for (int i = floor(minIY); i <= ceil(maxIY); ++i) iY_.push_back(i);

        vector<int> iX_t(iX_.begin(), iX_.end());
        vector<int> iY_t(iY_.begin(), iY_.end());

        for (size_t i = 0; i < iX_.size(); ++i) {
            for (size_t j = 0; j < iY_.size(); ++j) {
                double dd[2] = {zmin[0] - obsk[0], zmin[1] - obsk[1]};
                double normer = sqrt(dd[0] * dd[0] + dd[1] * dd[1]);
                int i1 = normer < gMap[iX_[i] + (iY_[j] - 1) * n1];
                gMap[iX_[i] + (iY_[j] - 1) * n1] = normer;
            }
        }
    }

    // 返回结果
    GridMap result;
    result.hMap = vector<int>(400 * 400 * 50, 0);
    result.flagkxcar = vector<int>(400 * 400 * 50, 0);
    return result;
}

int main() {
    // 主程序逻辑（根据需求进行测试）

    return 0;
}
